/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/publicInterfaceSrc/public_kernel.cpp
 * @Description  : 导航系统的公共接口实现。
 *                 其主要功能包括：AGV运动模式判断、激光导航数据获取、位姿信息获取、自动预检查、任务管理、导航控制等。
 *                 通过ROS与外部进行通信，涉及到地图加载、路径规划、速度控制、任务状态管理等多个方面。
 *                 实现了多个导航和控制相关的功能函数，提供了AGV的导航模式切换、任务管理、路径设置等接口。
 *                 代码中的函数通过与ROS节点进行交互，获取导航参数、位姿数据、控制指令等信息，实现对AGV的导航控制。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:12:56
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include "public_kernel.h" 
#include "world.h"
#include "opInclude/operation.h" // for peripheral
#include <agv_srvs/srv/set_navigation_mode.hpp>
#include <agv_msgs/msg/output_port.hpp>
#include "check_manager.h"
#include "tf2/utils.h"
#include "tools.h"

using namespace std;
constexpr double POSE_NAV_TIME_OUT = 150.0f;  // 150ms 正常导航时的数据超时时间
constexpr double POSE_MAX_TIME_OUT = 3000.0f; // 3000ms 上线时允许大一些的超时时间

// 手自动模式获取比特位(agv_info)
constexpr int MANUL_BIT = 4;
constexpr int HALF_BIT = 22;
constexpr int AUTO_ONLINE_BIT = 23;

constexpr char MAP_FILE_NAME[] = "/map/mapTest.xml";
// 仿真模式开启软件定时器
//  #ifdef AGV_DEBUG_NAV
#define AGV_USE_DEBUG_TIMER // 使用软定时器
// #endif

// 查询位宏定义
#define BIT_(x) (((uint64_t)1) << (x))
#define TestBit_(x, i) ((x & BIT_(i)) != 0)
#define SET_BIT_(x, n) (x | ((uint64_t)(1) << n))
#define CLEAR_BIT_(x, n) (x & ~((uint64_t)(1) << n))

World world;                        // 地图
Timer timer;                        // 定时器
TaskManager TaskManager;            // 任务管理器
WorkStatusRecord VhcStatus(&world); // 状态记录
PublicAgvData PublicData;           // 全局数据
std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::TwistStamped>> AgvVelPub;           // 全局发布者
///////////////////////////////////////////////////////////////////////////////
//   Magnetic navigation system interface.                                   //
///////////////////////////////////////////////////////////////////////////////
extern "C"
{

//////////////////////////////////////////////////////////////////////////////
/**
 * @brief 判断是否为全向运动模式
 *
 * 该函数用于判断当前AGV模型是否为全向运动模式。
 *
 * @return 如果当前AGV模型为全向运动模式，则返回true；否则返回false。
 */
bool IsOmniDirection() {
    return MotionParm::agv_model_ == static_cast<int>(AgvModel::model_steer);
}

/**
 * @brief 判断AGV是否有舵轮
 *
 * 判断AGV是否有舵轮。
 *
 * @return 如果AGV有舵轮，则返回true；否则返回false。
 */
bool HasSteer() {
    return MotionParm::agv_model_ == static_cast<int>(AgvModel::model_steer) || 
            MotionParm::agv_model_ == static_cast<int>(AgvModel::model_single_steer);
}

/**
 * @brief 判断激光导航是否存在
 *
 * 该函数用于判断当前是否存在激光导航。
 *
 * @return 如果存在激光导航，则返回true；否则返回false。
 */
bool LaserNavExist() {
    return TRUE;
}

/**
 * @brief 获取当前导航类型
 *
 * 从ROS参数服务器中获取AGV的导航类型。
 *
 * @return 返回导航类型的整数值。如果参数不存在，则返回-1。
 */
int GetCurNavType() {
    int type = ROSNode()->get_parameter_or("AGVParam.agv_navigation_type", -1);
    return type;
}

/**
 * @brief 获取SLAM定位得分
 *
 * 获取最新一次的SLAM定位得分，并将其赋值给引用参数score。
 *
 * @param score 用于存储SLAM得分的引用参数
 *
 * @return 成功获取SLAM得分时返回true，否则返回false
 */
bool GetSlamScore(float &score) {
    bool ret = PublicData.agv_pose_data.laser_score.IsNew(500);
    auto data = PublicData.agv_pose_data.laser_score.Get();
    if (ret) {
        score = data.score;
    }
    return ret;
}

/**
 * @brief 获取激光自动定位信息
 *
 * 获取当前AGV的激光自动定位信息，包括x坐标、y坐标、方向角theta和质量评分。
 *
 * @param x x坐标的引用，用于存储返回的x坐标值
 * @param y y坐标的引用，用于存储返回的y坐标值
 * @param theta 方向角的引用，用于存储返回的方向角值
 * @param quality 质量评分的引用，用于存储返回的质量评分值
 * @param allow_timeout 是否允许超时，默认为true
 * @param out_log 是否输出日志，默认为false
 *
 * @return 如果定位数据有效且二维码初始化成功，则返回true；否则返回false
 */
bool GetLaserAutoPos(float& x, float& y, float& theta, float& quality, bool allow_timeout, bool out_log) {
    std::lock_guard<std::mutex> locker(PublicData.agv_pose_data.pose_mutex);
    // #2 二维码导航模式上线初始化判断
    bool qr_init = true;
    double time_pose = 0.0;
    geometry_msgs::msg::Pose cur_pose;

    if (GetCurNavType() == agv_srvs::srv::SetNavigationMode_Request::QR_CODE) {
        qr_init = PublicData.agv_pose_data.real_pose.qr_init;
        if (!qr_init) {
            DEBUG_ERROR_OUT("QR init failed; qr_init:" << qr_init);
        }
    }

    auto time_header = PublicData.agv_pose_data.real_pose.header.stamp;
    time_pose = time_header.sec + time_header.nanosec / 1e9;
    cur_pose = PublicData.agv_pose_data.real_pose.pose.pose;

    // #3 判断定位数据的有效性，如果超时 则为无效数据
    const double time_out = allow_timeout ? POSE_MAX_TIME_OUT : POSE_NAV_TIME_OUT;
    // double timePose = PublicData.agv_pose_data.real_pose.header.stamp.seconds();
    double time_now = ROSTime().seconds();
    double time_diff_ms = (time_now - time_pose) * 1000.0;
    DEBUG_OUT("pose: time_diff_ms: " << time_diff_ms);

    x = cur_pose.position.x;
    y = cur_pose.position.y;
    theta = 0.0f;
    try {
        theta = tf2::getYaw(cur_pose.orientation);
    } catch (...) {
        theta = 0.0f;
        DEBUG_ERROR_OUT("tf2::getYaw(cur_pose.orientation) failed");
        return false;
    }

    if (std::isnan(x) || std::isnan(y) || std::isnan(theta)) {
        quality = -100.0f;
        DEBUG_ERROR_OUT("error: isnan: real pos bValid: set false clear data is -8888"
                        << ": x:" << x << " y:" << y << " t:" << theta << ",fQuality:" << quality);
        y = x = -8888;
        theta = 0;
        return false;
    }

    PublicData.agv_pose_data.is_valid = time_diff_ms < time_out;
    quality = PublicData.agv_pose_data.real_pose.score; // PublicData.agv_pose_data.fMatchRate;

    if (!PublicData.agv_pose_data.is_valid) {
        quality = -100.0f;
        if (std::abs(y) < 0.0001f && std::abs(x) < 0.0001f) {
            y = x = -8888;
        }
       
        DEBUG_ERROR_OUT("location data(pose) time out:timeMsDiff=" << time_diff_ms << ",fQuality=" << quality
                                                                   << ",fX=" << x << ",fY=" << y);
    }

    // #4 实时记录位姿信息
    static int times = 0;
    if (out_log) {
        DEBUG_OUT("real pos bValid:" << PublicData.agv_pose_data.is_valid << ": x:" << x << " y:" << y << " t:" << theta << ",fQuality:" << quality);
    } else {
        if (times++ >= 2) {
            times = 0;
            DEBUG_OUT("times++>2 : auto pose: real pos bValid:" << PublicData.agv_pose_data.is_valid << ": x:" << x << " y:" << y << " t:" << theta << ",fQuality:" << quality);
        }
    }

    return qr_init  && PublicData.agv_pose_data.is_valid;
}

/**
 * @brief 获取AGV的位姿信息
 *
 * 从公共数据结构中获取AGV的位姿信息，并检查其有效性。
 *
 * @param ret_pose 存储返回的位姿信息
 * @return 如果位姿信息有效，则返回true；否则返回false
 */
bool GetPose(agv_msgs::msg::PoseWithConfidence& ret_pose) {
    bool valid = false;
    std::lock_guard<std::mutex> locker(PublicData.agv_pose_data.pose_mutex);
    auto pose = PublicData.agv_pose_data.real_pose;
    auto time_header = pose.header.stamp;
    double time_pose = time_header.sec + time_header.nanosec / 1e9;
    double time_now = ROSTime().seconds();
    double time_diff_ms = (time_now - time_pose) * 1000.0f;
    valid = time_diff_ms < POSE_NAV_TIME_OUT;
    ret_pose  = pose;
    return valid;
}

/**
 * @brief 获取当前运行的节点信息
 *
 * 获取当前车辆状态中的起始节点和结束节点信息，并将这些信息赋值给传入的引用参数。
 *
 * @param from_node 起始节点的引用，用于存储起始节点的信息
 * @param to_node 结束节点的引用，用于存储结束节点的信息
 */
void GetCurRunNode(USHORT& from_node, USHORT& to_node) {
    from_node = VhcStatus.from_node_;
    to_node = VhcStatus.to_node_;
}

/**
 * @brief 自动预检查函数
 *
 * 该函数用于进行自动预检查，根据激光扫描仪获取的位置和姿态信息，判断是否满足初始化条件，
 * 若满足条件则设置初始状态并设置路径位数据。
 *
 * @return 若成功设置初始状态并设置路径位数据，则返回true；否则返回false。
 */
bool AutoPreCheck() {
    float x{0.0}, y{0.0}, theta{0.0}, q{0.0f};
    // bool bLoadSqlite = LoadInitState();// unsigned char g{0};
    if (GetLaserAutoPos(x, y, theta, q, true, true) && q > 5.0f) {
        bool ret = SetInitStateByPosture(x, y, theta);
        if (ret) {
            FirstSetPathBitsData();
            DEBUG_OUT("AutoPreCheck OK");
        }
        return ret;
    }
    return false;
}

/**
 * @brief 根据节点ID设置初始状态
 *
 * 该函数根据给定的起始节点ID和目标节点ID，调用VhcStatus的StaticLocation函数来设置静态位置，
 * 然后调用TaskManager的SetInitState函数来设置初始状态。
 *
 * @param at_node_id 起始节点ID
 * @param to_node_id 目标节点ID
 * @return 如果设置成功，返回0；如果设置失败，返回-3
 */
int SetInitStateByNode(USHORT at_node_id, USHORT to_node_id) {
    bool ret = VhcStatus.StaticLocation(at_node_id, to_node_id, false);
    if (!ret) {
        return -3;
    }
    TaskManager.SetInitState(VhcStatus);
    return 0;
}

/**
 * @brief 根据姿态设置初始状态
 *
 * 根据给定的位置（x, y）和朝向（theta）设置车辆的初始状态。
 *
 * @param x 车辆的X坐标
 * @param y 车辆的Y坐标
 * @param theta 车辆的朝向（弧度）
 *
 * @return 如果成功设置初始状态，则返回true；否则返回false。
 */
bool SetInitStateByPosture(float x, float y, float theta) {
    Posture pst(x, y, theta);
    float max_dev_x = 0.35f;     // 35cm   AGV.LaserNavSys.m_fMaxDevX;
    float max_dev_theta = 0.34f; // 15degree AGV.LaserNavSys.m_fMaxDevThita;
    ProjectData *project_data = VhcStatus.FindLocByPosture(pst, max_dev_x, max_dev_theta);
    if (project_data == NULL) {
        DEBUG_OUT("Error: laser find path by posture is failed: is null");
        return FALSE;
    } else if (fabs(project_data->x_) > max_dev_x || abs(Angle(project_data->theta_)) > max_dev_theta) {
        DEBUG_OUT("Error: laser find path by posture is failed: dev is too big");
        return FALSE;
    } else {
        DEBUG_OUT("OK: laser find path by posture is ok");
        TaskManager.SetInitState(VhcStatus);
        return TRUE;
    }

    return TRUE;
}

/**
 * @brief 加载初始化状态
 *
 * 加载初始化状态，并返回操作结果。
 *
 * @return 如果成功加载状态，则返回true；否则返回false。
 */
bool LoadInitState() {
    return VhcStatus.LoadFromSqlite();
}

/**
 * @brief 开始导航
 *
 * 根据参数决定是否启动导航功能，并设置相关参数。
 *
 * @param start 是否启动导航功能
 * @param online 是否使用在线地图
 */
void StartNav(bool start, bool online) {
    PublicData.cur_action_state_list.Clear();
    // 设置激光避障检测是否开启 自动：开启 其他：关闭
    SetReportAutoMode(start);
    // 设置使能二维码ID检测
    SingleCheckObj->SetEnable(start);

    if (start) {
        SetAgvToStop();
        TaskManager.StartNavigation(true, online);
    } else {
        TaskManager.StartNavigation(false, online);
        SetAgvToStop();
    }
}

/**
 * @brief 设置暂停预编译标识
 *
 * 该函数用于设置暂停预编译标识。
 *
 * @param pause 是否暂停预编译的标志，true 表示暂停编译，false 表示不暂停编译
 */
void SetPausePreCompile(bool pause) {
    TaskManager.SetPauseCompileFlag(pause);
}

/**
 * @brief 强制更新当前位置
 *
 * 该函数通过调用 GetLaserAutoPos 获取激光传感器的自动位置，并更新当前动态虚拟位置。
 *
 * @return 如果成功更新位置，则返回 true；否则返回 false。
 */
bool ForceUpdataCurPos() {
    float x = 0, y = 0, t = 0, q = 0;
    bool ret = GetLaserAutoPos(x, y, t, q, false, true);
    if (ret) {
        Posture pst;
        pst.x_ = x;
        pst.y_ = y;
        pst.theta_ = t;
        TaskManager.UpdataCurDynVirtulPos(pst);
    }

    return ret;
}

/**
 * @brief 添加离线订单任务
 *
 * 将订单任务添加到任务管理器中，以便进行离线处理。
 *
 * @param order_task_req 订单任务请求对象
 *
 * @return 如果成功添加任务，则返回true；否则返回false。
 */
bool AddOfflineOrderTask(const agv_srvs::srv::LocalOrder::Request& order_task_req) {
    return TaskManager.OnSendOfflineTask(order_task_req);
}

/**
 * @brief 添加在线订单任务
 *
 * 将订单任务添加到任务管理器中，并尝试将其发送到相应的处理流程。
 *
 * @param order_task 订单任务信息，类型为 agv_msgs::msg::Order
 *
 * @return 如果任务成功添加到任务管理器并发送，则返回 true；否则返回 false
 */
bool AddOnlineOrderTask(const agv_msgs::msg::Order& order_task) {
    return TaskManager.OnSendOnlineOrderTask(order_task);
}

/**
 * @brief 获取控制周期
 *
 * 返回控制周期的值，单位为秒。
 *
 * @return 返回控制周期的值，默认为0.05秒。
 */
float GetCtrlCycle() {
    return 0.05f;
}

/**
 * @brief 初始化主定时器
 *
 * 该函数用于初始化主定时器，并设置其周期。
 *
 * @return 初始化结果
 *         - 0：初始化成功
 *         - -1：设置定时器周期失败
 *         - -2：启动定时器失败
 */
int InitMainTimer() {// nStep = 5
    if (!timer.SetPeriod(round(GetCtrlCycle() * 1000.0f))) { // 0.05s to 50ms
        return -1;
    }

#ifdef AGV_USE_DEBUG_TIMER // in Linux, if debug, use the AGV_LINUX_DEBUG_TIMER
    if (!timer.Start(TRUE)) {
        return -2;
    }
#else
    if (!timer.Start()) {
        return -2;
    }
#endif
    return 0;
}

/**
 * @brief 强制设置任务暂停状态
 *
 * 该函数用于强制设置任务的暂停状态。
 *
 * @param pause 任务暂停状态，true 表示暂停，false 表示继续执行
 */
void ForceSetPauseTask(bool pause) {
    PublicData.task_pause = pause;
    DEBUG_WARN_OUT("ForceSetPauseTask:pause=" << pause);
}

/**
 * @brief 取消当前任务
 *
 * 此函数用于取消当前任务。
 *
 * @return 返回任务是否被成功取消
 *         - true: 任务被成功取消
 *         - false: 任务取消失败
 */
bool CancelCurTask() {
    return PublicData.task_cancel; //= false;
}

/**
 * @brief 设置任务取消状态
 *
 * 设置PublicData中的task_cancel成员变量为指定的取消状态，并输出调试信息。
 *
 * @param cancel 是否取消任务，true表示取消任务，false表示不取消任务
 */
void SetCancelTask(bool cancel) {
    PublicData.task_cancel = cancel; //= false;
    DEBUG_OUT("SetCancelTask:" << cancel);
}

/**
 * @brief 判断是否已经暂停任务
 *
 * 根据当前任务的取消状态、任务暂停标志位以及停止位发生标志位，判断是否已经暂停任务。
 *
 * @return 如果已经暂停任务，则返回true；否则返回false。
 */
bool ReadyPauseTask() {
    bool ret = false;
    bool cancel_task = CancelCurTask();
    bool task_pause = PublicData.task_pause;
    bool stop_bit = PublicData.stopBitOccur.Occur();
    if (cancel_task || task_pause || stop_bit) {
      ret = true;
      DEBUG_OUT("Set AGV stop: ReadyPauseTask: cancel_task=" << 
            cancel_task << ", task_pause=" << task_pause <<", stop_bit=" << stop_bit);
    }
    return ret;
}

/**
 * @brief 事件触发函数
 *
 * 获取当前事件的动作类型。
 *
 * @return 返回当前事件的动作类型（uint8_t类型）。
 */
uint8_t EventOccur() {
    // 2023.12.20 modify by haibo
    agv_msgs::msg::AGVEvent event = PublicData.cur_event.Get();
    return event.action;
}

// 2023.1.5 add by haibo
float Getvellimit() {
    // 2024.1.5 add by haibo
    agv_msgs::msg::AGVEvent event = PublicData.cur_event.Get();
    return event.vellimit;
}

/**
 * @brief 判断并返回其他停止信号的状态
 *
 * 根据传入的转向命令数据，判断是否存在其他停止信号，并返回对应的状态值。
 *
 * @return 返回其他停止信号的状态值，
 *         - DoSteerState::SteerNormal: 正常状态
 *         - DoSteerState::SteerError: 出现转向错误
 *         - DoSteerState::Steering: 正在转向
 *         - DoSteerState::SteerBrakeLock: 刹车锁定
 */
int OtherStopSignal() {
    int ret = static_cast<int>(DoSteerState::SteerNormal);
    auto &data = PublicData.steer_cmd.msg_fb_;
    bool steer_err = data.IsNew(150) && data.Get().steer_err;
    bool steering = data.IsNew(150) && data.Get().steering;
    bool brake_lock = data.IsNew(150) && data.Get().chassis_not_ready;
    if (steer_err || steering || brake_lock) {
        DEBUG_WARN_OUT("OtherStopSignal occur; id=" << data.Get().id << "steering=" << data.Get().steering << 
                        ", steer_err=" << data.Get().steer_err << ", brake_lock=" << data.Get().chassis_not_ready);
    }
    
    if (steer_err) {
        ret = static_cast<int>(DoSteerState::SteerError);
    } else if (steering) {
        ret = static_cast<int>(DoSteerState::Steering);
    } else if (brake_lock) {
        ret = static_cast<int>(DoSteerState::SteerBrakeLock);
    } else {
        ret = static_cast<int>(DoSteerState::SteerNormal);
    }
    return ret;
}

/**
 * @brief 检查是否存在其他慢速停止信号
 *
 * 该函数检查是否存在慢速停止信号，即是否因为重新定位停止或启动延迟而被标记为慢速停止。
 *
 * @return 如果存在慢速停止信号，则返回true；否则返回false。
 */
bool OtherSlowStopSignal() {
    bool slow_stop = PublicData.stop_signal.relocate_stop || PublicData.stop_signal.start_delay;
    if (slow_stop) {
        DEBUG_WARN_OUT("OtherSlowStopSignal: relocate_stop=" << PublicData.stop_signal.relocate_stop << 
                        ", start_delay=%d" << PublicData.stop_signal.start_delay);
    }
    return slow_stop;
}

/**
 * @brief 设置停止类别
 *
 * 根据给定的停止类别和发生情况，更新停止信号的状态。
 *
 * @param type 停止类别，可以是 STOP_RELOCATE 或 STOP_START_DELAY
 * @param occur 发生情况，true 表示发生，false 表示未发生
 */
void SetStopCategory(StopType type, bool occur) {
    bool changed = false;
    auto &signal = PublicData.stop_signal;
    switch (type) {
        case StopType::STOP_RELOCATE:
            if (occur != signal.relocate_stop) {
                signal.relocate_stop = occur;
                changed = true;
            }
            break;
        case StopType::STOP_START_DELAY:
            if (occur != signal.start_delay) {
                signal.start_delay = occur;
                changed = true;
            }
            break;
        default:
            break;
    }

    if (changed) {
      DEBUG_OUT("SetStopCategory type:" << (int)type << ",occur:" << occur);
    }
}

/**
 * @brief 将AGV设置为停止状态
 *
 * 将AGV设置为停止状态，将速度设置为0，并发布停止指令。
 *
 * @return 无返回值
 */
void SetAgvToStop() {
    PublicData.ctrl_out_vel.twist.linear.x = 0;
    PublicData.ctrl_out_vel.twist.linear.y = 0;
    PublicData.ctrl_out_vel.twist.angular.z = 0;

    uint32_t num = ROSNode()->count_subscribers("cmd_vel");
    bool valid = num > 0;
    if (!valid) {
        DEBUG_WARN_OUT("SetAgvToStop failed: pub is not valid:num=" << (int)(num));
        return;
    }

    AgvVelPub->publish(PublicData.ctrl_out_vel);
    DEBUG_OUT("setAgvToStop");
}

/**
 * @brief 设置自动模式下的导航速度
 *
 * 设置自动模式下的线性速度（x轴和y轴）和角速度（z轴）。
 *
 * @param linear_x x轴上的线性速度
 * @param linear_y y轴上的线性速度
 * @param angular_z z轴上的角速度
 */
void SetAutoModeNavVel(const float& linear_x, const float& linear_y, const float& angular_z) {
    PublicData.ctrl_out_vel.twist.linear.x = linear_x;
    PublicData.ctrl_out_vel.twist.linear.y = linear_y;
    PublicData.ctrl_out_vel.twist.angular.z = angular_z;
    DEBUG_OUT("SetAutoModeAgvVel:fLinearX=" << linear_x << ",fLinearY=" << linear_y << ",fAngularZ=" << angular_z);
}

/**
 * @brief 清除导航节点ID记录
 *
 * 该函数用于清除导航系统中的节点ID记录，将VhcStatus结构体中的from_node_和to_node_成员变量重置为65535，
 * 并输出一条调试信息表示已经执行了一次清除操作。
 */
void ClearNavNodeIdRecord() {
    // clear last pos
    VhcStatus.from_node_ = 65535;
    VhcStatus.to_node_ = 65535;
    DEBUG_OUT("ClearNavNodeIdRecord once");
}

/**
 * @brief 加载地图
 *
 * 加载地图文件，并获取地图ID
 *
 * @return 如果加载成功返回true，否则返回false
 */
bool LoadMap() {
    // #1 load map
    //  wrte to yaml  pose
    bool load = false;
    try { // PACKAGE_NAME
        // std::string map_file = ros::package::getPath(PACKAGE_NAME) + MAP__FILE_NAME;
        std::string map_file = std::string(getenv("HOME")) + MAP_FILE_NAME;
        load = world.LoadMap(map_file);
        VhcStatus.map_id_ = world.GetMapId();
        if (load) {
            DEBUG_OUT("LoadMap is ok");
        } else {
            DEBUG_ERROR_OUT("LoadMap falied");
        }
    } catch (...) {
        /// std::cerr << e.what() << '\n';
        DEBUG_OUT("error: LoadMap is error ; it is crash");
    }
    return load;
}

/**
 * @brief 根据给定的AGV模式状态，返回相应的工作模式
 *
 * 根据输入的AGV模式状态（如手动、半自动、自动等），确定相应的工作模式，并返回该模式对应的整数值。
 *
 * @param state AGV模式状态，取值来自agv_msgs::msg::AGVMode枚举
 * @return 返回相应的工作模式整数值
 */
uint64_t AnalysisMode(uint8_t state) {
    uint64_t new_work_mode = 0;
    bool manul_bit = state == agv_msgs::msg::AGVMode::MANUAL;
    bool half_auto_bit = state == agv_msgs::msg::AGVMode::SEMI_AUTOMATIC;
    bool auto_online_bit = state == agv_msgs::msg::AGVMode::AUTOMATIC;
    new_work_mode = manul_bit ? SET_BIT_(new_work_mode, 0) : new_work_mode;
    new_work_mode = half_auto_bit ? SET_BIT_(new_work_mode, 1) : new_work_mode;
    if (new_work_mode == 0 && auto_online_bit) {
        new_work_mode = static_cast<uint64_t>(AgvWorkMode::mod_auto_online);
    }
    return new_work_mode;
}

/**
 * @brief 初始化自动任务定时器
 *
 * 该函数用于初始化自动任务定时器，确保后续可以自动执行定时任务。
 *
 * @return 如果初始化成功返回true，否则返回false
 */
bool InitAutoTaskTimer() {
    int ret = InitMainTimer();
    if (ret == 0) {
        DEBUG_OUT("Init timer thread is ok");
    } else {
        DEBUG_ERROR_OUT("Init timer thread is failed: ret:" << ret);
    }
    return ret == 0;
}

/**
 * @brief 初始化运动参数
 *
 * 该函数用于初始化运动参数，调用MotionParm类的InitParam方法。
 *
 * @param node ROS节点句柄，用于从ROS参数服务器获取参数
 */
void InitMotionParam() {
    MotionParm::InitParam();
}

/**
 * @brief 初始化节点运动参数
 *
 * 该函数用于初始化节点的运动参数，包括设置当前工作模式为手动模式，并初始化运动参数。
 *
 * @param node ROS节点句柄，用于与ROS系统交互
 */
void InitNodeMotionParam() {
    PublicData.cur_work_mode = AgvWorkMode::mod_manual; // mod_manual; mod_auto
    SetCancelTask(false);
    InitMotionParam();
    // // 初始开启二维码检测线程
    // SingleCheckObj->Init();
}

///////////////order action/////////////////
// #    instant : yes
// #	startPause stopPause startCharging stopCharging initPosition
// #	stateRequest logReport cancelOrder factsheetRequest
// #    instant : no
// #	pick drop detectObject finePositioning waitForTrigger

/**
 * @brief 即时动作处理
 *
 * 根据传入的即时操作列表，执行相应的操作。
 *
 * @param instant_actions 即时操作列表
 * @return 如果所有操作都成功执行，则返回true；否则返回false
 */
bool HandleInstantActions(std::vector<agv_msgs::msg::Action> instant_actions) {
    bool ret = true;
    for (const auto &action : instant_actions) {
      if (action.actiontype == "startPause") {
            ForceSetPauseTask(true);
            DEBUG_OUT("StartPause task ok");
      } else if (action.actiontype == "stopPause") {
            ForceSetPauseTask(false);
            DEBUG_OUT("StopPause task ok");
      } else if (action.actiontype == "cancelOrder") {
            SetCancelTask(true);
            DEBUG_OUT("Cancel order task ok");
      } else if (action.actiontype == "stopCharging") {
            SetStopCharging(true);
            DEBUG_OUT("StopCharging order task ok");
      } else if (action.actiontype == "vehicleControl") {
            SetVehicleControl(action);
            DEBUG_OUT("VehicleControl order task ok");
      } else {
            ret = false;
            DEBUG_ERROR_OUT("Error: unknown instant action: actiontype:" << action.actiontype);
      }
    }
    return ret;
}

/**
 * @brief 根据动作和状态设置动作状态
 *
 * 根据传入的动作和状态字符串，生成一个新的动作状态对象并返回。
 *
 * @param action 输入的动作对象
 * @param status 输入的状态字符串
 * @return 返回一个新的动作状态对象
 */
agv_msgs::msg::ActionState SetActionState(const agv_msgs::msg::Action& action, const std::string& status) {
    agv_msgs::msg::ActionState new_state;
    new_state.actionid = action.actionid;
    new_state.actiontype = action.actiontype;
    new_state.actionstatus = status;
    return new_state;
}

/**
 * @brief 更新动作状态ID
 *
 * 根据传入的new_id和update_id更新PublicData.cur_action_state_list中的动作状态ID。
 *
 * @param new_id 新的动作状态ID
 * @param update_id 关联的动作ID
 */
void UpdateActionStateId(long long new_id, uint32_t update_id) {
    bool same = new_id == PublicData.cur_action_state_list.order_id_;
    if (!same) {
        PublicData.cur_action_state_list.Clear();
    }
    PublicData.cur_action_state_list.SetId(new_id, update_id);
}

/**
 * @brief 更新动作状态列表
 *
 * 根据传入的新状态更新动作状态列表。如果列表中已存在相同动作ID和动作类型的状态，则更新该状态；否则，将新状态添加到列表中。
 *
 * @param new_state 要更新的新动作状态
 * @return 如果更新了列表中已存在的状态，则返回true；否则，返回false。
 */
bool UpdateActionListStates(agv_msgs::msg::ActionState new_state) {
    bool ret = false;
    auto &state_list = PublicData.cur_action_state_list;
    for (auto &state : state_list.states_) {
        if (state.actionid == new_state.actionid && state.actiontype == new_state.actiontype) {
            state = new_state;
            ret = true;
            break;
        }
    }

    if (!ret) {
        state_list.states_.push_back(new_state);
        DEBUG_OUT("Insert a new action state: actionid:" << 
                    new_state.actionid << ", actiontype:" <<new_state.actiontype);
    }
    return ret;
}

//////////////////////////开启预告运行时数据线程//////////////////////////
/**
 * @brief 初始化报告线程
 *
 * 初始化报告线程，用于发送运行数据报告。
 *
 * @param node ROS节点句柄指针
 *
 * @return 如果初始化成功，则返回true；否则返回false
 */
bool InitReportThread() {
    return PublicData.report_run_data.Init();
}

/**
 * @brief 设置报告自动模式
 *
 * 该函数用于设置报告是否自动运行。
 *
 * @param enable 是否启用自动模式，true表示启用，false表示禁用
 * @return 如果设置成功，返回true；否则返回false
 */
bool SetReportAutoMode(bool enable) {
    return PublicData.report_run_data.SetAutoMode(enable);
}

/**
 * @brief 设置导航路径点字段属性
 *
 * 设置导航路径点字段属性。
 *
 * @param word1 第一个字段
 * @param word2 第二个字段
 */
void SetNavPathPointWordData(const int &word1, const int &word2) {
    PublicData.report_run_data.SetPathPointWordsProperty(word1, word2);
}

/**
 * @brief 设置导航路径字段属性
 *
 * 该函数用于设置导航路径的字段属性。
 *
 * @param word1 第一个字段
 * @param word2 第二个字段
 */
void SetNavPathWordData(const int &word1, const int &word2) {
    PublicData.report_run_data.SetPathWordsProperty(word1, word2);
}

/**
 * @brief 设置导航路径位属性
 *
 * 设置导航路径的位属性。
 *
 * @param bits 要设置的位属性
 */
void SetNavPathBitsData(const uint16_t &bits) {
    PublicData.report_run_data.SetPathBitsProperty(bits);
}

/**
 * @brief 设置导航路径场景属性
 *
 * 设置导航路径中的场景属性。
 *
 * @param scenes 场景属性数据
 */
void SetNavPathScenesData(const std::string &scenes) {
    PublicData.report_run_data.SetPathScenesProperty(scenes);
}

/**
 * @brief 获取运行路径属性
 *
 * 该函数用于获取当前运行路径的属性信息。
 *
 * @return 返回包含运行路径属性的 RunPathProperty 对象。
 */
RunPathProperty GetRunPathProperty() {
    return PublicData.report_run_data.GetPathProperty();
}

/**
 * @brief 设置导航路径位数据（首次），避障使用
 *
 * 此函数用于设置导航路径的位数据。首先，通过调用 VhcStatus 对象的 FindCurBits 方法获取当前位数据，
 * 然后调用 SetNavPathBitsData 函数将这些位数据设置为导航路径数据。
 * 最后，通过 DEBUG_OUT 输出调试信息。
 */
void FirstSetPathBitsData() {
    uint16_t bits = VhcStatus.FindCurBits();
    SetNavPathBitsData(bits);
    DEBUG_OUT("FirstSetPathBitsData:"
              << ",bits=" << bits);
}

/////////////////////////////////Set Nav Mode ///////////////////////////////////////////
/**
 * @brief 更改导航模式
 *
 * 根据传入的模式值，更改当前导航模式。
 *
 * @param mode USHORT类型，表示要设置的导航模式
 */
void ChangeNavMode(USHORT mode) {
    PublicData.report_run_data.SetCurGuideMode(mode);
}

/////////////////////////////////Set Traj Type ///////////////////////////////////////////
/**
 * @brief 修改当前轨迹类型
 *
 * 将当前轨迹类型修改为指定的类型。
 *
 * @param type 要设置的轨迹类型
 */
void ChangeTrajType(int type) {
    PublicData.report_run_data.SetCurTrajType(type);
}

////////////////////////CHARING////////////////////////
/**
 * @brief 设置停止充电状态
 *
 * 设置是否停止充电。
 *
 * @param charging 是否停止充电。true 表示停止充电，false 表示继续充电。
 */
void SetStopCharging(bool charging) {
    PublicData.stop_charging = charging; //= false;
    DEBUG_OUT("SetChargingTask:" << charging);
}

/**
 * @brief 获取是否停止充电的标志
 *
 * 此函数用于获取一个布尔值，表示当前是否已停止充电。
 *
 * @return 如果当前已停止充电，则返回 true；否则返回 false。
 */
bool GetStopCharging() {
    return PublicData.stop_charging;
}

/**
 * @brief 设置充电IO口的开关状态(不再控制充电继电器)
 *
 * 该函数用于设置充电IO口的开关状态。
 *
 * @param open 充电IO口的开关状态
 *              - true: 打开充电IO口
 *              - false: 关闭充电IO口
 *
 * @return 返回bool类型值
 *         - true: 设置成功
 *         - false: 设置失败（例如，当函数内部决定不再控制充电继电器时）
 */
bool SetChargeIo(bool open) { 
    // 不再控制充电继电器
    return false;
}

/**
 * @brief 获取规划控制速度
 *
 * 返回当前规划控制输出的速度值。
 *
 * @return 返回类型为geometry_msgs::msg::Twist的引用，表示控制速度
 */
geometry_msgs::msg::TwistStamped &GetControlVel() {
    return PublicData.ctrl_out_vel;
}

/**
 * @brief 设置车辆控制参数
 *
 * 根据传入的 agv_msgs::msg::Action 结构体设置车辆的控制参数。
 *
 * @param action 包含车辆控制参数的 agv_msgs::msg::Action 结构体
 */
void SetVehicleControl(agv_msgs::msg::Action action) {
    // 查看是否设置需要延迟退出
    auto params = action.actionparameters;
    auto &cotrol = PublicData.instant_control;
    if (params.empty()) {
        DEBUG_OUT("SetVehicleControl params is empty;");
        return;
    }
    // PublicData.instant_control
    for (auto param : params) {
        if (param.key == "loadStatus") {
            cotrol.virtual_load = param.value == "LOADED"; // if (string2Number(param.value, ))
            DEBUG_OUT("virtualLoad=" << cotrol.virtual_load);
            ROSNode()->set_parameter({"virtualLoad", (int)cotrol.virtual_load});
        }
        DEBUG_OUT("SetVehicleControl:param=(" << param.key << "," << param.value <<")");
    }
}

/**
 * @brief 获取当前工作模式
 *
 * 获取当前的工作模式。
 *
 * @return 返回当前的工作模式。
 */
AgvWorkMode GetCurMode() {
    return PublicData.cur_work_mode;
}

} // end extern "C"
